#!/usr/bin/env python
import rospy
import time
import math
from geometry_msgs.msg import PoseWithCovarianceStamped

class nav_init:
    def __init__(self) :
        rospy.init_node('nav_init')
        self.setpose_pub = rospy.Publisher("initialpose",PoseWithCovarianceStamped,latch=True, queue_size=1)
        #self.setpose_pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped,queue_size=10)
        self.set_pose = {'x':0,'y':-2,'theta':math.pi/2}
        self.test_set_pose_flag = True
        self.test_set_pose_cnt = 100000

        while self.test_set_pose_flag == True:

            self.set_inital_pose()
            self.test_set_pose_cnt -= 1
            if self.test_set_pose_cnt == 0:
                self.test_set_pose_flag = False
            rospy.sleep(0.01)

    def set_inital_pose(self):
        # Define a set inital pose publisher.
        rospy.loginfo("start set pose...")
        p = PoseWithCovarianceStamped()
        p.header.stamp = rospy.Time.now()
        p.header.frame_id = "map"
        p.pose.pose.position.x = self.set_pose['x']
        p.pose.pose.position.y = self.set_pose['y']
        p.pose.pose.position.z = 0
        (p.pose.pose.orientation.x,
        p.pose.pose.orientation.y,
        p.pose.pose.orientation.z,
        p.pose.pose.orientation.w) = 0,0,math.sin(math.pi/4),math.cos(math.pi/4)
        p.pose.covariance[6 * 0 + 0] = 0.1 * 0.1
        p.pose.covariance[6 * 1 + 1] = 0.1 * 0.1
        p.pose.covariance[6 * 3 + 3] = math.pi / 120.0 * math.pi / 120.0

        self.setpose_pub.publish(p)
nav_init()